using System;
using System.Collections.Generic;
using System.Text;

namespace Diversity.Robotics.Navigation
{
    public class TrigUtils
    {
        private double _twoPi = Math.PI*2;
        public TrigUtils()
        {
        }

        /// <summary>
        /// Find a represntation of the angle in the +/- Pi range
        /// </summary>
        /// <param name="angle">The angle you want to clean</param>
        /// <returns>An angle between -Pi and +Pi</returns>
        public double CleanAngle(double angle)
        {
            if (angle > Math.PI)
            {
                return -(_twoPi - angle);
            }
            else if (angle < -Math.PI)
            {
                return angle + _twoPi;
            }
            else
            {
                return angle;
            }
        }

        /// <summary>
        /// Compares two angles for difference
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns>The absolute difference</returns>
        public double GetDifference(double a, double b)
        {
            if (a >= 0 && b >= 0)
            {
                return Math.Abs(a - b);
            }
            else if (a < 0 && b < 0)
            {
                return Math.Abs(Math.Abs(a) - Math.Abs(b));
            }
            else
            {
                return Math.Abs(Math.Abs(a) + Math.Abs(b)); // was clean angle
            }
        }
    }
}
